/*
 * GPS_PID.h
 *
 *  Created on: Feb 9, 2014
 *      Author: ppks
 */

#ifndef GPS_PID_H_
#define GPS_PID_H_


/********************************************************************************/
/* Types Definition                                                             */
/********************************************************************************/
typedef struct PID_PARAM_ {
    float kP;
    float kI;
    float kD;
    float Imax;
} PID_PARAM;

typedef struct GPS_PID_ {
    float   integrator;     /* integrator value */
    int32_t last_input;     /* last input for derivative */
    float   lastderivative; /* last derivative for low-pass filter */
    float   output;
    float   derivative;
} PID;

/********************************************************************************/
/* Exported Variables Declaration                                               */
/********************************************************************************/
extern PID_PARAM poshold_PID_PARAM;
extern PID_PARAM posholdRate_PID_PARAM;
extern PID_PARAM nav_PID_PARAM;

extern PID posholdPID[2];
extern PID posholdRatePID[2];
extern PID navPID[2];

/********************************************************************************/
/* Exported Functions Prototypes                                                */
/********************************************************************************/
extern void GPS_SetPIDs(void);
extern void GPS_ResetPID(struct GPS_PID_* pid);
extern int32_t GPS_GetPID_P(int32_t error, struct PID_PARAM_* pid);
extern int32_t GPS_GetPID_I(int32_t error, float* dt, struct GPS_PID_* pid, struct PID_PARAM_* pid_param);
extern int32_t GPS_GetPID_D(int32_t input, float* dt, struct GPS_PID_* pid, struct PID_PARAM_* pid_param);
#endif /* GPS_PID_H_ */
